
#include "PIControlTask.h"


#include "FreeRTOS.h"
#include "task.h"

#include "nxbot.h"

#include "pwm.h"
#include "motors.h"

static void PIControlTask(void *pvParameters);

void executePIControlTask(general *pvParameters)
{

	xTaskCreate(PIControlTask,(const signed portCHAR * const)"PIControlTask",PIControlTask_stackSize,pvParameters,tskIDLE_PRIORITY + 3,( xTaskHandle * ) NULL);
}

//Tarea que se encarga de implementar el control de velocidad. NO esta terminada, porq falta implementar la funcion controller, que espero salga este fincho.
static void PIControlTask(void *pvParameters)
{	
	//convertion of parameters 
	general *globals;
	globals=(general *)pvParameters;

	unsigned int freq;
	portTickType actualTick;

	/*int vias=0;
	int vel=0;*/
	int leftError=0;
	int rightError=0;
	
	/*int velLeftRef=globals->gVels.velLeftRef;
	int velRightRef=globals->gVels.velRightRef;
	int velRightAct=globals->gVels.velRightAct;
	int velLeftAct=globals->gVels.velLeftAct;*/
	
	float kp=globals->gPiParam.Kp;
	//float ki=globals->gPiParam.Ki;
	
	//Calculating errors
	leftError=(globals->gVels.velLeftRef)-(globals->gVels.velLeftAct);
	rightError=(globals->gVels.velRightRef)-(globals->gVels.velRightAct);
		
	actualTick = xTaskGetTickCount();
	for(;;)
	{
	
		//obtain the frequency of the PIControl 
		taskENTER_CRITICAL();
		freq=globals->gTimes.intervalPI;
		
		
		
		taskENTER_CRITICAL();
		
		//setVelMotor((signed char)leftError*kp,MOTOR_LEFT);
		//setVelMotor((signed char)rightError*kp,MOTOR_RIGHT);
		setVelMotor((signed char)globals->gVels.velLeftRef,MOTOR_LEFT);
		setVelMotor((signed char)globals->gVels.velRightRef,MOTOR_RIGHT);
		taskEXIT_CRITICAL();
		
		
		//time delay according to the irSensorsInterval value
		
		vTaskDelayUntil( &actualTick,freq);




	}
}
